

#define BOOST_TEST_MODULE MMM_MODEL_READER_XML_TEST

#include <MMM/Model/Model.h>
#include <MMM/Motion/Legacy/LegacyMotion.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <string>

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

BOOST_AUTO_TEST_CASE(testParseEmptyString)
{
	const std::string modelString = "";
	MMM::ModelReaderXMLPtr r(new MMM::ModelReaderXML());
	MMM::ModelPtr m = r->createModelFromString(modelString);
	BOOST_REQUIRE(!m);
}

BOOST_AUTO_TEST_CASE(testParseLoadModel)
{
	const std::string modelString = 
		"<?xml version='1.0' encoding='UTF-8'?>"
		"<Robot Type='DemoModel' RootNode='Joint1'>"
		"	<RobotNode name='Joint1'>"
		"		<Visualization enable='true'>"
		"			<File type='Inventor'>test.iv</File>"
		"			<CoordinateAxis type='Inventor' enable='true' scaling='1' text='Axis1'/>"
		"		</Visualization>"
		"		<CollisionModel>"
		"			<File type='Inventor'>test.iv</File>"
		"		</CollisionModel>"
		"		<Child name='Joint2'/>"
		"	</RobotNode>"
		"	<RobotNode name='Joint2'>"
		"		<Transform>"
		"			<Translation x='100' y='0' z='0' units='mm'/>"
		"		</Transform>"
		"		<Joint type='revolute' offset='0'>"
		"			<Limits unit='degree' lo='-90' hi='45'/>"
		"			<Axis x='0' y='0' z='1'/>"
		"		</Joint>"
		"		<Visualization enable='true'>"
		"			<File type='Inventor'>test.iv</File>"
		"		</Visualization>"
		"		<CollisionModel>"
		"			<File boundingbox='true' type='Inventor'>test.iv</File>"
		"		</CollisionModel>"
		"	</RobotNode>"
		"</Robot>";
	MMM::ModelReaderXMLPtr r(new MMM::ModelReaderXML());
	MMM::ModelPtr m = r->createModelFromString(modelString);
	BOOST_REQUIRE(m);
	BOOST_REQUIRE(m->getRoot()=="Joint1");
	BOOST_REQUIRE(m->getName()=="DemoModel");
	BOOST_REQUIRE(m->getModels().size()==2);
	MMM::ModelNodePtr mod1 = m->getModels()[0];
	BOOST_REQUIRE(mod1);
	BOOST_REQUIRE(mod1->name=="Joint1");
	BOOST_REQUIRE(mod1->joint.jointType==MMM::eFixed);
	MMM::ModelNodePtr mod2 = m->getModels()[1];
	BOOST_REQUIRE(mod2);
	BOOST_REQUIRE(mod2->name=="Joint2");
	BOOST_REQUIRE(mod2->joint.jointType==MMM::eRevolute);
	BOOST_REQUIRE(mod2->joint.axis.isApprox(Eigen::Vector3f(0,0,1.0f)));

	// check export and re-load data
	std::string modelString2 = m->toXML();
	MMM::XML::addXMLHeader(modelString2);
	//std::cout << modelString2 << std::endl;
	MMM::ModelPtr m2 = r->createModelFromString(modelString2);
	BOOST_REQUIRE(m2);
	BOOST_REQUIRE(m2->getRoot()=="Joint1");
	BOOST_REQUIRE(m2->getName()=="DemoModel");
	BOOST_REQUIRE(m2->getModels().size()==2);

	mod1 = m2->getModels()[0];
	BOOST_REQUIRE(mod1);
	BOOST_REQUIRE(mod1->name=="Joint1");
	BOOST_REQUIRE(mod1->joint.jointType==MMM::eFixed);
	mod2 = m2->getModels()[1];
	BOOST_REQUIRE(mod2);
	BOOST_REQUIRE(mod2->name=="Joint2");
	BOOST_REQUIRE(mod2->joint.jointType==MMM::eRevolute);
	BOOST_REQUIRE(mod2->joint.axis.isApprox(Eigen::Vector3f(0,0,1.0f)));
}

// BOOST_AUTO_TEST_SUITE_END()
